/*************************************************************************
*
*	simulate_heading.c
*
*  Copyright 2007,2008 Steve Stancliff
*
*  Part of the TAOSF SSA system.
*
*  This module provides functions for simulating boat status.
*
*
**************************************************************************
*
*  version 1.0.8, 10/17/08, SBS
*   - broken out from simulate_boat
*
*  version 1.0.7, 10/4/08, SBS
*   - documentation
*
*  version 1.0.6, 8/27/08, SBS
*   - use status_SetTimeString()
*   - add random offset to boat start positions
*
*  version 1.0.5, 5/30/08, SBS
*    - move conversions (lat, long, deg, rad, etc) into conversions.h
*    - add conditionals for simulating buoys
*    - cleanup
*
*  version 1.0.4, 5/29/08, SBS
*    - add UpdateBoatSpeed()
*    - add status "direction" = net motion direction
*    - use direction instead of heading in positionfromheadingspeed
*    - move InitBoat here from simulate_engineering
*
*  version 1.0.3, 5/18/08, SBS
*    - add boat rotating into wind during free drift
*
*  version 1.0.2, 5/17/08, SBS
*    - modify drift to include both wind and water 
*    - add turn rate to heading changes
*
*  version 1.0.1, 5/12/08, SBS
*    - add drift
*
*  version 1.0, 1/13/08, SBS
*    - split from simulate_engineering.c
*
**************************************************************************
*
*  This program is free software; you can redistribute it and/or modify
*  it under the terms of the GNU General Public License as published by
*  the Free Software Foundation; either version 2 of the License, or
*  (at your option) any later version.
*
*  This program is distributed in the hope that it will be useful, but 
*  WITHOUT ANY WARRANTY; without even the implied warranty of 
*  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 
*  General Public License for more details.
*
*  You should have received a copy of the GNU General Public License 
*  along with this program; if not, write to the Free Software Foundation, 
*  Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 
*
**************************************************************************/
/**
*  @file 
*  @ingroup plat_eng_simulate
*  @brief
*  This module provides functions for simulating boat status.
*  <hr>
**/

#include <stdio.h>
#include <stdlib.h>
#include <time.h>
#include <math.h>

#include "waypoints.h"
#include "platform_types.h"
#include "platform_modes.h"
#include "status.h"
//#include "conversions.h"

//#include "../initialize.h" 

#include "science_main.h"

//#include "boatmodes.h"

#include "simulate_heading.h"




// contributions of water current and wind to drift
//#define DRIFT_FROM_WATER 0.4
//#define DRIFT_FROM_WIND  0.1
//#define DRIFT_FROM_WATER 0.
//#define DRIFT_FROM_WIND  0.

// rotational rate for boat sitting still (deg/s)
#define HDGROTINC 1.5

// turn rate for moving boat (deg/s)
#define HDGTURNRATE 12.0



/********************************************************
*
* UpdateBoatHeading()
*/
/**
* @brief
* Determine new boat heading
*
********************************************************/
void UpdateBoatHeading_sim(
			STATUS *s
			, int deltat
			, double TargetHdg)
{
	static double dHdgRotInc = HDGROTINC;		// drift rotation rate
	static double dHdgTurnRate = HDGTURNRATE;	// powered turn rate
	static double tgtHdg; // goal heading
	static double deltaHdg;	// difference between goal and current headings
	static double turnDeg; // how much boat has rotated since last update
	static SCI_DBL sd3,sd4; // used to store wind data
	static POSITION p1;


	//**********
	// determine heading change rate
	//**********
	
	if((TargetHdg < 500.) // nonsense value of 512 is used to indicate no target
			&& (s->CurrWpt.dSpeedMps > 0)){ // boat is under power
			
		tgtHdg = TargetHdg;
		
		deltaHdg = tgtHdg - s->dHdgDeg;
		
		turnDeg = dHdgTurnRate * deltat;
		

	}else{// when free drifting, rotate boat into wind
		
		// retrieve wind vector
		p1 = status_PositionFromStatus(*s);
		sd3 = science_main(WINDSPEED,p1);
		sd4 = science_main(WINDANGLE,p1);
	
		tgtHdg = sd4.dValue + 180.;  // want _into_ wind instead of _with_
		if(tgtHdg >= 360.){
			tgtHdg -= 360.;
		}	
		
		deltaHdg = tgtHdg - s->dHdgDeg;
		
		turnDeg = dHdgRotInc * deltat * (sd3.dValue / 2);
		
	}
	
//	printf("\ntarget heading = %f deg.",tgtHdg);
	
	

	//**********
	// determine new heading from rate and time
	//**********
	if( fabs(deltaHdg) < turnDeg){	// if closer to goal than update 
	
		s->dHdgDeg = tgtHdg;	// then set heading = goal heading
	
	}else{	// determine direction of rotation and increment by turn rate * time
	
		if( (deltaHdg) > 0.){
			if(deltaHdg < 180.){
				
				s->dHdgDeg += turnDeg;
				
			}else{
				
				s->dHdgDeg -= turnDeg;
			}
		}else{
			if(deltaHdg < -180.){
				
				s->dHdgDeg += turnDeg;
				
			}else{
				
				s->dHdgDeg -= turnDeg;
			}
		}	
	}


	if(s->dHdgDeg >= 360.){
		s->dHdgDeg -= 360.;
	}else if(s->dHdgDeg < 0.){
		s->dHdgDeg += 360.;
	}
	
	return;
}


